import can
import time
import datetime
import random


class VirtualCANBus:
    def __init__(self, bus_name='virtual_can0'):
        self.bus = can.interface.Bus(
            interface='virtual',
            channel=bus_name,
            bitrate=500000,
            receive_own_messages=True
        )
        self.message_handlers = []
        print(f"虚拟CAN总线 {bus_name} 已初始化")

    def send_message(self, arbitration_id, data):
        msg = can.Message(arbitration_id=arbitration_id, data=data)
        try:
            self.bus.send(msg)
            timestamp = datetime.datetime.now().strftime("%H:%M:%S.%f")[:-3]
            print(f"[{timestamp}] TX: ID=0x{arbitration_id:03X}, DLC={len(data)}, Data={data.hex(' ').upper()}")
        except can.CanError as e:
            print(f"发送失败: {e}")

    def register_handler(self, handler):
        self.message_handlers.append(handler)
        print(f"已注册处理器: {handler.__name__}")

    def receive_messages(self):
        while True:
            msg = self.bus.recv(0)
            if msg is None:
                break

            timestamp = datetime.datetime.now().strftime("%H:%M:%S.%f")[:-3]
            print(
                f"[{timestamp}] RX: ID=0x{msg.arbitration_id:03X}, DLC={len(msg.data)}, Data={msg.data.hex(' ').upper()}")

            for handler in self.message_handlers:
                handler(msg)


class VirtualECU:
    def __init__(self, name, can_bus, ecu_id):
        self.name = name
        self.can_bus = can_bus
        self.ecu_id = ecu_id
        self.diagnostic_session = 0
        self.last_status_time = 0
        self.status_interval = 2.0

        # UDS服务处理函数
        self.uds_services = {
            0x10: self.handle_session_control,
            0x22: self.handle_read_data,
            0x2E: self.handle_write_data,
            0x3E: self.handle_tester_present
        }

        can_bus.register_handler(self.handle_message)
        print(f"虚拟ECU {name} (ID=0x{ecu_id:X}) 已初始化")

    def handle_message(self, msg):
        if msg.arbitration_id != self.ecu_id:
            return

        if len(msg.data) == 0:
            return

        if msg.data[0] == 0x01:  # 状态消息
            return

        service_id = msg.data[0]
        print(f"[ECU {self.name}] 处理服务: 0x{service_id:02X}")

        if service_id in self.uds_services:
            self.uds_services[service_id](msg)
        else:
            self.send_negative_response(service_id, 0x11)

    def handle_session_control(self, msg):
        if len(msg.data) >= 2 and msg.data[1] in [0x01, 0x02, 0x03]:
            self.diagnostic_session = msg.data[1]
            self.can_bus.send_message(self.ecu_id + 8, bytes([0x50, self.diagnostic_session]))
            print(f"[ECU {self.name}] 会话切换: {self.diagnostic_session}")
        else:
            self.send_negative_response(0x10, 0x12)

    def handle_read_data(self, msg):
        if len(msg.data) < 3:
            self.send_negative_response(0x22, 0x13)
            return

        data_id = (msg.data[1] << 8) | msg.data[2]
        if data_id == 0xF190:  # 示例标识符
            value = random.randint(0, 100)
            self.can_bus.send_message(self.ecu_id + 8, bytes([0x62, 0xF1, 0x90, value]))
        else:
            self.send_negative_response(0x22, 0x31)

    def handle_write_data(self, msg):
        if len(msg.data) < 4:
            self.send_negative_response(0x2E, 0x13)
            return

        self.can_bus.send_message(self.ecu_id + 8, bytes([0x6E, msg.data[1], msg.data[2]]))

    def handle_tester_present(self, msg):
        self.can_bus.send_message(self.ecu_id + 8, bytes([0x7E]))
        print(f"[ECU {self.name}] 测试器在线确认")

    def send_negative_response(self, service_id, error_code):
        self.can_bus.send_message(self.ecu_id + 8, bytes([0x7F, service_id, error_code]))
        print(f"[ECU {self.name}] 否定响应: 0x{service_id:02X}, 错误 0x{error_code:02X}")

    def update(self):
        current_time = time.time()
        if current_time - self.last_status_time > self.status_interval:
            self.can_bus.send_message(self.ecu_id, bytes([0x01, self.diagnostic_session] + [0] * 6))
            print(f"[ECU {self.name}] 发送状态")
            self.last_status_time = current_time


class CANTransmitter:
    def __init__(self, can_bus, tx_id, interval_ms=1000):
        self.can_bus = can_bus
        self.tx_id = tx_id
        self.interval_ms = interval_ms
        self.last_send_time = 0

    def send_diagnostic_request(self, service_id, data=None):
        if data is None:
            data = []
        self.can_bus.send_message(self.tx_id, bytes([service_id] + data))
        print(f"[发送器] 诊断请求: 0x{service_id:02X}, 数据 {bytes([service_id] + data).hex(' ').upper()}")
        time.sleep(0.1)  # 确保处理时间

    def update(self):
        current_time = time.time()
        if current_time - self.last_send_time > self.interval_ms / 1000.0:
            counter = int(current_time * 1000) % 65536
            self.can_bus.send_message(self.tx_id,
                                      bytes([counter & 0xFF, (counter >> 8) & 0xFF, 0xAA, 0xBB, 0xCC, 0xDD]))
            self.last_send_time = current_time


if __name__ == "__main__":
    # 初始化系统组件
    can_bus = VirtualCANBus()
    engine_ecu = VirtualECU("Engine_ECU", can_bus, 0x7E0)
    transmitter = CANTransmitter(can_bus, 0x7E0, 1000)

    # 运行标志
    running = True

    # 主循环
    try:
        print("系统运行中 (Ctrl+C停止)...")
        while running:
            # 处理消息
            can_bus.receive_messages()

            # 更新组件
            engine_ecu.update()
            transmitter.update()

            # 添加诊断请求示例
            current_time = time.time()
            if int(current_time) % 5 == 0:  # 每5秒发送诊断请求
                if int(current_time) % 15 == 0:
                    transmitter.send_diagnostic_request(0x3E)  # 测试器在线
                elif int(current_time) % 10 == 0:
                    transmitter.send_diagnostic_request(0x22, [0xF1, 0x90])  # 读取数据
                else:
                    transmitter.send_diagnostic_request(0x10, [0x01])  # 会话控制

            # 短暂休眠
            time.sleep(0.01)

    except KeyboardInterrupt:
        print("\n程序终止")
    finally:
        print("资源清理完成")